#pragma once
#include <string>
#include "common_lib/log.h"
#include "control/fsm/fsm_base.h"

#include "task_msgs/task_msg.h"
#include "control/control_params.h"
#include "control/custom_class.h"
#include "common_lib/common.h"

namespace FSM
{
class MoveFsm : public FSMBase<MoveFsm, amr_trajectory_msgs::amr_trajectory_msg, control::AMRState>
{
public:
	void Init() override;
	
	void EventHandle(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state) override;

	//获取当前在执行的任务id
	int GetTaskId();

	// 获取当前执行任务开始时间
	ros::Time GetTaskTime();
	
	//设置Finish条件中的到点条件阈值
	void InitFinishPoseThreshold(float tofinish_dis_precision, float tofinish_angle_precision);

	// 是否设置强制结束任务
	void SetForceFinish(bool flag){ is_force_to_finish_ = flag;}
private:
	void StateHandle(const std::string &state, control::AMRState &amr_state) override;

	void FinishState(control::AMRState &amr_state);
	void WorkState(control::AMRState &amr_state);
	void PauseState(control::AMRState &amr_state);
	void FailState(control::AMRState &amr_state);


	bool IsFinishToWork(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);

	bool IsWorkToFinish(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);
	bool IsWorkToPause(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);
	bool IsWorkToFail(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);

	bool IsPauseToWork(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);
	bool IsPauseToFail(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);

	bool IsFailToWork(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state);

private:
	int task_id_ = 0;
	//任务开始执行时间
	ros::Time task_time_;
	//故障诊断上次的状态
	control::Event_e last_event;
	// 急停按钮
	bool last_stop_bnt = false;

	// 状态机到点距离精度
	float fsm_tofinish_dis_precision_;
	// 状态机到点角度精度
	float fsm_tofinish_angle_precision_;

	// 强制结束任务
	bool is_force_to_finish_ = false;

	common_lib::TON ton_amr_still_;
};
} // namespace FSM
